// Include files.
#include <exception>
#include "NetworkCommunication/UsageReporting.h"
#include "VersionPrinter.hpp"
#include "FpkException.hpp"
#include "SystematicRobot.hpp"

using namespace std;

/* The following variable definition is derived from 
 * WPILib, which is licenced under the licence specified
 * in "Licence-WPILib.txt".
 */
/**
 *  The default time per period.
 *  
 *  Will be 0 if it waits until the driver station sends data.
 */
const double SystematicRobot::sDefaultPeriod = 0.0;

/* The following code in this function is derived from 
 * WPILib, which is licenced under the licence specified
 * in "Licence-WPILib.txt".
 */
/**
 *  Constructor for SystematicRobot.
 */
SystematicRobot::SystematicRobot() :
	m_mode(ModeDisabled), 
	m_period(sDefaultPeriod)
{
	m_watchdog.SetEnabled(false);
	SetPeriod(sDefaultPeriod);
}

/**
 *  Destructor for SystematicRobot.
 */
SystematicRobot::~SystematicRobot()
{
}

void SystematicRobot::StartCompetition()
{
	Run();
}

/**
 *  Your robot's subsystems and controllers should be added here.
 *  
 *  Users should override this method to add their robot's subsystems and controllers.
 */
// Define this as an empty function.
void SystematicRobot::Initialize()
{
}

/**
 *  Returns a list of RobotSubsystems.
 *  
 *  @return A vector of subsystems currently in robot.
 */
const set<RobotSubsystem*> &SystematicRobot::GetSubsystems()
{
	return m_subsystems;
}

/**
 *  Add another RobotSubsystem.
 *  
 *  @param subsystem The subsystem to add to the robot.
 */
void SystematicRobot::AddSubsystem(RobotSubsystem &subsystem)
{
	m_subsystems.insert(&subsystem);
}

/**
 *  Add another RobotSubsystem.
 *  
 *  @param subsystem The subsystem to add to the robot.
 */
void SystematicRobot::AddSubsystem(RobotSubsystem *subsystem)
{
	m_subsystems.insert(subsystem);
}

/**
 *  Returns a set of RobotControllers.
 *  
 *  @return A set of controllers currently in robot.
 */
const set<RobotController*> &SystematicRobot::GetControllers()
{
	return m_controllers;
}

/**
 *  Add another RobotController.
 *  
 *  @param controller The controller to add to the robot.
 */
void SystematicRobot::AddController(RobotController &controller)
{
	m_controllers.insert(&controller);
	controller.HandleAddedToRobot(*this);
}

/**
 *  Add a RobotController to be activate when and only when the robot is enables.
 *  
 *  @param controller The controller to add to the robot.
 */
void SystematicRobot::AddEnabledController(RobotController &controller)
{
	m_controllerEnabled.AddController(controller);
}

/**
 *  Set the autonomous controller.
 *  There can only be one autonomous controller.
 *  Any previous autonomous controller's set will no longer be controlled by systematic robot.
 *  
 *  @param controller The autonomous controller for the robot.
 */
void SystematicRobot::SetAutonomousController(RobotController& controller)
{
	m_controllerAutonomous = &controller;
}

/**
 *  Set the teleop controller.
 *  There can only be one teleop controller.
 *  Any previous teleop controller's set will no longer be controlled by systematic robot.
 *  
 *  @param controller The teleop controller for the robot.
 */
void SystematicRobot::SetTeleopController(RobotController& controller)
{
	m_controllerTeleop = &controller;
}

/**
 *  Gets the time between each periodic loop or 0 if it waits until the driver station sends data.
 *  
 *  Almost exactly like IterativeRobot::GetPeriod().
 *  
 *  @returns The time between each periodic loop or 0 if it waits until the driver station sends data.
 */
double SystematicRobot::GetPeriod()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	return m_period;
}

/**
 *  Set the time between each periodic loop or 0 for it to wait until the driver station sends data.
 *  
 *  Almost exactly like IterativeRobot::SetPeriod().
 *  
 *  @param period The time between each periodic loop or 0 for it to wait until the driver station sends data.
 */
void SystematicRobot::SetPeriod(double period)
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	if (period > 0.0)
	{
		// Not syncing with the DS, so start the timer for the main loop
		m_timer.Reset();
		m_timer.Start();
	}
	else
	{
		// Syncing with the DS, don't need the timer
		m_timer.Stop();
	}
	
	m_period = period;
}

/**
 *  Gets the number of periodic loops per second.
 *  
 *  Almost exactly like IterativeRobot::GetLoopsPerSec().
 *  
 *  @returns The number of periodic loops per second.
 */
double SystematicRobot::GetLoopsPerSec()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	if (m_period <= 0.0)
		return 50.0;
	return 1.0 / m_period;
}

void SystematicRobot::Run()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	nUsageReporting::report(nUsageReporting::kResourceType_Framework, nUsageReporting::kFramework_Iterative);
	
	VersionPrinter::GetEntechFpkVersionPrinter().PrintVersion(4);
	
	AddController(m_controllerEnabled);
	
	Initialize();
	HandleSubsystemsInitalization();
	
	m_watchdog.SetExpiration(1);
	m_watchdog.SetEnabled(true);
	
	try
	{
		while (true)
		{
			m_watchdog.Feed();
			
			if (IsDisabled())
			{
				if (m_mode != ModeDisabled)
				{
					HandleSubsystemsDisabled();
					
					m_controllerEnabled.Deactivate();
					m_controllerAutonomous->Deactivate();
					m_controllerTeleop->Deactivate();
					
					m_mode = ModeDisabled;
					
					printf("Robot: Disabled\n");
				}
				
				if (GetIsNextPeriodReady())
				{
					FRC_NetworkCommunication_observeUserProgramDisabled();
					HandleSubsystemsDisabled();
					HandleControllersPeriodic();
					DelayTask();
				}
				
				HandleControllersContinuous();
			}
			else
			{
				if (m_mode == ModeDisabled)
				{
					HandleSubsystemsEnabled();
					m_controllerEnabled.Activate();
				}
				
				if (IsOperatorControl() && m_mode != ModeTeleop)
				{
					m_controllerAutonomous->Deactivate();
					m_controllerTeleop->Activate();
					
					m_mode = ModeTeleop;
					
					printf("Robot: Teleop\n");
				}
				else if (IsAutonomous() && m_mode != ModeAutonomous)
				{
					m_controllerTeleop->Deactivate();
					m_controllerAutonomous->Activate();
					
					m_mode = ModeAutonomous;
					
					printf("Robot: Autonomous\n");
				}
				
				if (GetIsNextPeriodReady())
				{
					if (IsAutonomous())
						FRC_NetworkCommunication_observeUserProgramAutonomous();
					else if (IsOperatorControl())
						FRC_NetworkCommunication_observeUserProgramTeleop();
					
					HandleControllersPeriodic();
					HandleSubsystemsPeriodic();
					DelayTask();
				}
				
				HandleControllersContinuous();
				HandleSubsystemsContinuous();
			}
		}
	}
	catch (FpkException e)
	{
		printf("Catching Fpk Exception!  %s\n", e.what());
		printf("Throwing back in to the water...\n");
		throw e;
	}
	catch (exception &e)
	{
		printf("Catching Normal Exception!");
		printf(e.what());
		printf("\n");
		printf("Throwing back in to the water...\n");
		throw e;
	}
}

bool SystematicRobot::GetIsNextPeriodReady()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	if (m_period > 0.0)
		return m_timer.HasPeriodPassed(m_period);
	else
		return m_ds->IsNewControlData();
}

void SystematicRobot::DelayTask()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	taskDelay(1);
}

/*
 * Allow subsystems to run initialization code.
 */
void SystematicRobot::HandleSubsystemsInitalization()
{
	set<RobotSubsystem*>::iterator it;
	
	for (it = m_subsystems.begin(); it != m_subsystems.end(); ++it)
	{
		(*it)->HandleInitialize();
	}
}

/*
 * Start all the subsystems.
 */
void SystematicRobot::HandleSubsystemsEnabled()
{
	set<RobotSubsystem*>::iterator it;
	
	for (it = m_subsystems.begin(); it != m_subsystems.end(); ++it)
		(*it)->HandleEnable();
}

/*
 * Call all the subsystems' periodic functions.
 */
void SystematicRobot::HandleSubsystemsPeriodic()
{
	set<RobotSubsystem*>::iterator it;
	
	for (it = m_subsystems.begin(); it != m_subsystems.end(); ++it)
		(*it)->HandlePeriodic();
}

/*
 * Calls all subsystems' continuous functions.
 */
void SystematicRobot::HandleSubsystemsContinuous()
{
	set<RobotSubsystem*>::iterator it;
	
	for (it = m_subsystems.begin(); it != m_subsystems.end(); ++it)
		(*it)->HandleContinuous();
}

/*
 * Calls all the subsystems' disable functions.
 */
void SystematicRobot::HandleSubsystemsDisabled()
{
	set<RobotSubsystem*>::iterator it;
	
	for (it = m_subsystems.begin(); it != m_subsystems.end(); ++it)
		(*it)->HandleDisable();
}

/*
 * Calls all the controllers' periodic funtions.
 */
void SystematicRobot::HandleControllersPeriodic()
{
	set<RobotController*>::iterator it;
	
	for (it = m_controllers.begin(); it != m_controllers.end(); ++it)
	{
		if ((*it)->GetIsActive())
			(*it)->HandlePeriodic();
	}
}

/*
 * Calls all the controllers' continuous functions.
 */
void SystematicRobot::HandleControllersContinuous()
{
	set<RobotController*>::iterator it;
	
	for (it = m_controllers.begin(); it != m_controllers.end(); ++it)
	{
		if ((*it)->GetIsActive())
			(*it)->HandleContinuous();
	}
}
